#include "AStar.h"
#include <windows.h>

AStar::AStar(Node *source, Node *destination,Map &map):m_Map(map),m_ToExplore(*destination)
{
	unsigned int i,j;
	m_Source.SetAll(source);
	m_Destination.SetAll(destination);
	m_ToExplore.Clear();
	m_ToExplore.Push(source);
	
	m_IsVisited = new bool *[map.GetLines()];
	m_Sommets = new Node*[map.GetLines()];
	m_Solution = NULL;
	m_SolutionSize = 0;
	for(i = 0; i < m_Map.GetLines() ; i++)
	{
		m_IsVisited[i] = new bool[map.GetColumns()];
		for(j = 0 ; j < map.GetColumns() ; j++)
			m_IsVisited[i][j] = false;
		m_Sommets[i] = new Node [map.GetColumns()];
	}
}

AStar::~AStar()
{
	PartialDelete();
}

void AStar::PartialDelete() // vire tout sauf la map
{
	unsigned int i;
	
	m_ToExplore.Clear();
	for(i = 0; i < m_Map.GetLines() ; i++)
	{
		if(m_IsVisited != NULL && m_IsVisited[i] != NULL)
		{
			delete []m_IsVisited[i];
			m_IsVisited = NULL;
		}
		if(m_Sommets != NULL && m_Sommets[i] != NULL)
		{
			delete []m_Sommets[i];
			m_Sommets[i] = NULL;
		}
	}
	if(m_IsVisited != NULL)
	{
		delete []m_IsVisited;
		m_IsVisited = NULL;
	}
	if(m_Sommets != NULL)
	{
		delete []m_Sommets;
		m_Sommets = NULL;
	}
	if(m_Solution != NULL)
	{
		delete [] m_Solution;
		m_Solution = NULL;
	}
}



bool AStar::IsFinished()
{
	return m_ToExplore.IsFinished(m_Destination);
}

int AStar::Distance(int i1,int j1, int i2, int j2)
{
	return abs (j2 - j1) + abs(i2 - i1);
}

bool AStar::Compute()
{
	Node *nPtr,*tmp;
	unsigned int i = 0,posi,posj;
	const int dirX[4] = {1,-1,0,0};
	const int dirY[4] = {0,0,1,-1};
	LARGE_INTEGER freq,t1,t2;

	QueryPerformanceFrequency(&freq); 

	QueryPerformanceCounter(&t1);
	if(m_Source.GetX() == m_Destination.GetX() && m_Source.GetY() == m_Destination.GetY())
	{
		m_SolutionSize = 1;
		m_Solution = new Node *[2];
		m_Solution[0] = new Node(m_Source);
		m_Solution[1] = new Node(m_Source);
		m_Result = true;
		return true;
	}


	if(m_Map.GetElement(m_Destination.GetX(),m_Destination.GetY()) == 0 || m_Map.GetElement(m_Source.GetX(),m_Source.GetY()) == 0)
	{
		m_Result = false;
		return false;
	}
	m_Result = true;
	while(m_ToExplore.IsEmptyHeap() == false && IsFinished() == false)
	{
		nPtr = m_ToExplore.Pop(); // gets the lowest of the list
		for(i = 0 ; i < 4 ; i++)
		{
			posi = nPtr->GetX() + dirX[i]; // mira el vecino
			posj = nPtr->GetY() + dirY[i];
			if(posi < m_Map.GetLines() && posj < m_Map.GetColumns() && m_Map.GetElement(posi,posj) != 0) // si el vecino no sale del mapa
			{
		//		int d = 0;
				int d = Distance(posi,posj,m_Destination.GetX(),m_Destination.GetY());
				if(m_IsVisited[posi][posj] == false) // not visited
				{
					m_Sommets[posi][posj].SetAll(posi,posj,0,nPtr->GetCost()+m_Map.GetElement(posi,posj),&m_Sommets[nPtr->GetX()][nPtr->GetY()]);
					m_ToExplore.Push(&m_Sommets[posi][posj]); // add the node to the ToExplore list
					m_IsVisited[posi][posj] = true; // visited for further loops
				}
				else// visited
				{
					if(m_ToExplore.UpdateVisited(posi,posj,0,nPtr->GetCost()+m_Map.GetElement(posi,posj),&m_Sommets[nPtr->GetX()][nPtr->GetY()])==true) // si el coste es menor, actualizamos el nodo
						m_Sommets[posi][posj].SetAll(posi,posj,0,nPtr->GetCost() + m_Map.GetElement(posi,posj),&m_Sommets[nPtr->GetX()][nPtr->GetY()]);
				}
			} 
		}
	}
	if(m_ToExplore.IsEmptyHeap() == true && IsFinished() == false)// if the algorithm is over but destination hasn't been reached then no path
	{
		m_Result = false;
		return false;
	}

	// graba la solucion
	tmp = nPtr;
	m_SolutionSize = 2+nPtr->GetCost(); // 2 because soure and destination were not included
	m_Solution = new Node *[m_SolutionSize];
	m_Solution[0] = new Node(m_Destination);
	m_Cost = m_Sommets[m_Destination.GetX()][m_Destination.GetY()].GetCost();
	for(i = 0 ; i < nPtr->GetCost() ; i++)
	{
		m_Solution[i+1] = new Node(*tmp); 
		tmp = tmp->GetParent();
	}
	m_Solution[m_SolutionSize-1] = new Node(m_Source);
	QueryPerformanceCounter(&t2);
	printf("Time = %.6lf",double(t2.QuadPart - t1.QuadPart) / (double)freq.QuadPart);
	return true;
}